#include "auto_diff_base_node.h"


AutoDiffBase::AutoDiffBase(){
//  ROS_WARN("auto_diff_base_node");
  ros::NodeHandle node;
  ros::NodeHandle private_node("~");

  private_node.param<bool>("is_debug", is_debug_, false);
  private_node.param<bool>("publish_tf", publish_tf_, false);
  private_node.param<std::string>("imu_topic", imu_topic_, "");
  private_node.param<std::string>("joy_topic", joy_topic_, "");
  private_node.param<std::string>("hand_stick_cmd_topic", hand_stick_cmd_topic_, "");
  private_node.param<int>("enable_turbo_button", enable_turbo_button_, 5);
  private_node.param<std::string>("odom_topic", odom_topic_, "odom");
  private_node.param<std::string>("odom_frame", odom_frame_, "odom");
  private_node.param<std::string>("baselink_frame", baselink_frame_, "base_link");

  private_node.param<double>("wheel_diameter", wheel_diameter_, 150.0);
  private_node.param<double>("base_width", base_width_, 400.0);

  private_node.param<double>("max_x_velocity", max_x_velocity_, 1.2);
  private_node.param<double>("max_angular_velocity", max_angular_velocity_, 0.8);

  private_node.param<std::string>("port", port_, "");
  private_node.param<int>("baudrate", baudrate_, 115200);

  private_node.param<int>("odom_rate", odom_rate_, 20);

  //TODO: initialize application layer

  joy_sub_ = node.subscribe<sensor_msgs::Joy>(joy_topic_, 1, &AutoDiffBase::joyCallback, this);
  imu_sub_ = node.subscribe<sensor_msgs::Imu>(imu_topic_, 1, &AutoDiffBase::imuCallback, this);
  cmd_sub_ = node.subscribe<geometry_msgs::Twist>("/cmd_vel", 1, &AutoDiffBase::cmdVelCallback, this);
  hand_stick_cmd_sub_ = node.subscribe<geometry_msgs::Twist>(hand_stick_cmd_topic_, 1, &AutoDiffBase::handStickCmdVelCallback, this);

  odom_pub_ = node.advertise<nav_msgs::Odometry>(odom_topic_, 10);

  odom_timer_ = node.createTimer(ros::Duration(1.0 / odom_rate_), &AutoDiffBase::odomSendTimerCallback, this);
  cmd_send_timer_ = node.createTimer(ros::Duration(1.0 / 20), &AutoDiffBase::cmdSendTimerCallback, this);

  time_odom_old_ = ros::Time::now();
  nav_twist_time_ = hs_twist_time_ = ros::Time::now() - ros::Duration(5.0);
  twist_type_ = TwistType::NAV_CMD;

  //ROS_WARN_STREAM( "imu_topic" << ":" << imu_topic_ );
  ROS_INFO("Auto Diff Base Running.");
  ros::spin();
}

AutoDiffBase::~AutoDiffBase(){
   boost::mutex::scoped_lock lock1(mutex_);
}

void AutoDiffBase::imuCallback(const sensor_msgs::Imu::ConstPtr& imu_msg) {
    ROS_INFO_ONCE("imu received!");
  //boost::mutex::scoped_lock lock(imu_mutex);
    imu_ = *imu_msg.get();
    // imu_.header.stamp = imu_msg->header.stamp;
    // imu_.header.frame_id = imu_msg->header.frame_id;

    // imu_.orientation_covariance = imu_msg->orientation_covariance;
    // imu_.orientation = imu_msg->orientation;

    // imu_.angular_velocity = imu_msg->angular_velocity;

    // imu_.linear_acceleration = imu_msg->linear_acceleration;
}


void AutoDiffBase::joyCallback(const sensor_msgs::Joy::ConstPtr& joy_msg){
    ROS_INFO_ONCE("joy stick raw data received!");
    if (enable_turbo_button_ >= 0 &&
      joy_msg->buttons.size() > enable_turbo_button_ &&
      joy_msg->buttons[enable_turbo_button_]){  //user wants to use hand stick to control the robot
       twist_type_ = TwistType::HAND_STICK;
    }else {
       twist_type_ = TwistType::NAV_CMD;
    }
}

void AutoDiffBase::cmdVelCallback(const geometry_msgs::Twist::ConstPtr &msg){
//  if(twist_type_ != TwistType::NAV_CMD) return;
   nav_twist_ = *msg.get();
   nav_twist_time_ = ros::Time::now();
}

void AutoDiffBase::handStickCmdVelCallback(const geometry_msgs::Twist::ConstPtr &msg){
//  if(twist_type_ != TwistType::NAV_CMD) return;
   hs_twist_ = *msg.get();
   hs_twist_time_ = ros::Time::now();
}

void AutoDiffBase::odomSendTimerCallback(const ros::TimerEvent &){
    double roll, pitch, yaw;

    if(ros::Time::now() - imu_.header.stamp > ros::Duration(0.5)){
       ROS_WARN("Odom: waiting for imu data.");
       time_odom_old_ = ros::Time::now();
       twist_now_ = geometry_msgs::Twist();
       is_imu_ok_ = false;
       return;
    }else{
        is_imu_ok_ = true;
    }

    getRPY(imu_.orientation, roll, pitch, yaw);

    double dt = (ros::Time::now() - time_odom_old_).toSec();
    time_odom_old_ = ros::Time::now();

    int rpm_l = 0, rpm_r = 0;
    //TODO:get rpm from application layer
    double mps_l = 0, mps_r = 0;
    mps_l = rpm2mps(rpm_l);
    mps_r = rpm2mps(rpm_r);
    double mps = (mps_l + mps_r) / 2;

    double dx = mps * cos(yaw) * dt;
    double dy = mps * sin(yaw) * dt;

    odom_x_ += dx;
    odom_y_ += dy;

    if(is_debug_){
        ROS_WARN_STREAM(imu_.header.stamp << " " << imu_.header.frame_id);
        ROS_WARN("r=%f p=%f y=%f", roll*180.0/M_PI, pitch*180.0/M_PI, yaw*180.0/M_PI);
        ROS_WARN("ax=%f ay=%f az=%f", imu_.linear_acceleration.x, imu_.linear_acceleration.y, imu_.linear_acceleration.z);
        ROS_WARN("gx=%f gy=%f gz=%f", imu_.angular_velocity.x*180.0/M_PI, imu_.angular_velocity.y*180.0/M_PI, imu_.angular_velocity.z*180.0/M_PI);
        ROS_WARN("odom x=%f y=%f", odom_x_, odom_y_);
    }

    if(publish_tf_){
        geometry_msgs::TransformStamped odom_trans;
        odom_trans.header.stamp = imu_.header.stamp;
        odom_trans.header.frame_id = odom_frame_;
        odom_trans.child_frame_id = baselink_frame_;
        //robot's position in x,y, and z
        odom_trans.transform.translation.x = odom_x_;
        odom_trans.transform.translation.y = odom_y_;
        odom_trans.transform.translation.z = 0.0;

        //robot's heading in quaternion
        odom_trans.transform.rotation = imu_.orientation;

        //publish robot's tf using odom_trans object
        odom_broadcaster_.sendTransform(odom_trans);
    }

    nav_msgs::Odometry odom;
    odom.header.stamp = imu_.header.stamp;
    odom.header.frame_id = odom_frame_;

    //robot's position in x,y, and z
    odom.pose.pose.position.x = odom_x_;
    odom.pose.pose.position.y = odom_y_;
    odom.pose.pose.position.z = 0.0;
    
    //robot's heading in quaternion
    odom.pose.pose.orientation = imu_.orientation;

    //linear speed
    odom.child_frame_id = baselink_frame_;
    odom.twist.twist.linear.x = mps;
    odom.twist.twist.linear.y = 0.0;
    odom.twist.twist.linear.z = 0.0;
    
    //angular speed from IMU
    odom.twist.twist.angular.x = 0.0;
    odom.twist.twist.angular.y = 0.0;
    odom.twist.twist.angular.z = imu_.angular_velocity.z;

    twist_now_ = odom.twist.twist;

    //TODO: include covariance matrix here

    odom_pub_.publish(odom);
}


void AutoDiffBase::cmdSendTimerCallback(const ros::TimerEvent &){
    double ratio = 0.95;
   
    geometry_msgs::Twist twist;
    if(is_imu_ok_){
       if(twist_type_old_ != twist_type_){
            twist.linear.x = twist_now_.linear.x * ratio;
            twist.angular.z = twist_now_.angular.z * ratio;
            if(std::abs(twist.linear.x) < 0.05 && std::abs(twist.angular.z) < 0.05){
                twist_type_old_ = twist_type_;
            }
        }else{
            twist = twist_type_ == TwistType::NAV_CMD ? nav_twist_ : hs_twist_;
        }
    }else{

    }
    

    double mps = twist.linear.x;
    double angular_v = twist.angular.z;

    if(std::abs(mps) > max_x_velocity_){
        mps = sign(mps) * max_x_velocity_;
    }
    if(std::abs(angular_v) > max_angular_velocity_){
        angular_v = sign(angular_v) * max_angular_velocity_;
    }

    double dspd = angular_v * wheel_diameter_ / 2;
    double mps_l = mps - dspd;
    double mps_r = mps + dspd;

    int rpm_l = mps2rpm(mps_l);
    int rpm_r = mps2rpm(mps_r);

   if(is_debug_){
     ROS_WARN("rpm_l = %d rpm_r=%d twist_type_=%d mps=%.3f hs_t_x=%f", rpm_l, rpm_r, (int)twist_type_, mps, hs_twist_.angular.z);
   }
    //TODO: write rpm to the application layer
}




int main(int argc, char **argv) {
    ros::init(argc, argv, "auto_diff_base_node");

    AutoDiffBase *base = new AutoDiffBase();

    return 0;
}



